#include "StdAfx.h"
#include "PixelProcess.h"

//////////////////////////////////////////////////////////////////////

// zot: what should these values be?

// the learning rate for weights (0.0 -> 1.0)
#define alpha              0.01

// minimum portion of data accounted for by background (0.0 -> 1.0)
//  (1 - bgT) should be greater than (1 / K_MODELS)
//  otherwise all models could account for the background
#define bgT                0.79
//#define bgT                0.50

// initial weight should be pretty low
#define InitialWeight      0.02

// initial variance should be rather high
#define InitialDeviation   12.0
#define InitialVariance    ((InitialDeviation)*(InitialDeviation))

// a vector within <SigmaFactor> standard deviations of a model
//  is "explained" by that model
#define SigmaFactor        2.5
#define SFSquared          ((SigmaFactor)*(SigmaFactor))

UINT PixelModel::gid = 100L;

//////////////////////////////////////////////////////////////////////

PixelProcess::PixelProcess()
{
   int i, j;
   for(i=0; i<K_MODELS; i++)
   {
      model[i] = new PixelModel();
      
      model[i]->weight = InitialWeight;
      for(j=0; j<3; j++) model[i]->mean[j] = -999.0;
      model[i]->dev = 350.0;
      model[i]->var = model[i]->dev * model[i]->dev;
   }   

   NormalizeWeights();

   // again, we start by assuming everything is part of the background
   pmMatch = model[0];
   lastCategory = PC_Background;
}

//////////////////////////////////////////////////////////////////////

PixelProcess::~PixelProcess()
{
   int i;
   for(i=0; i<K_MODELS; i++) DeleteCleanA( model[i]);
}

//////////////////////////////////////////////////////////////////////

double PixelProcess::Gauss(PixelModel *pm)
{
   double v, a, b, det;
   double c1 = 15.74960994572242; // (2*pi)^1.5

   // calc the coefficient
   det = pm->var * pm->var * pm->var;
   a = 1.0 / (c1 * sqrt(det));

   // calc the exponent
   b = -0.5 * pm->mah2;

   v = a * exp(b);

   return v;
}

//////////////////////////////////////////////////////////////////////

PixelCategory PixelProcess::Process(BYTE *bgr)
{
   if (!bgr) return PC_Error;
   return Process(*(bgr+2), *(bgr+1), *(bgr));
}

//////////////////////////////////////////////////////////////////////


PixelCategory PixelProcess::ProcessByGradient(BYTE *p,BYTE *pRight,BYTE *pDown)
{
	

	
	
	return lastCategory;
}

PixelCategory PixelProcess::Process(BYTE cr, BYTE cg, BYTE cb)
{
   double v[3];
   lastPixelValue[0] = (double)cr;
   lastPixelValue[1] = (double)cg;
   lastPixelValue[2] = (double)cb;

   int i, j;
   double d, rho;

   // we want to check each model in order of ascending variance
   SortModelsByVariance();

   // see if the current pixel matches any of the models
   for(i=0; i<K_MODELS; i++)
   {
      // calculate the vector, v = (X - u)
      for(j=0; j<3; j++) v[j] = lastPixelValue[j] - model[i]->mean[j];

      // calculate the squared distance, d = |v|^2
      model[i]->dist2 = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];

      // zot: this is only equal to mahalanobis distance
      //   when covariance matrix = vI
      //   (v = scalar variance for all channels)
      model[i]->mah2 = model[i]->dist2 / model[i]->var;
      
      // see if X is close enough to this model
      if (model[i]->mah2 < SFSquared) break;
   }

   if (i<K_MODELS)
   {
      // we matched an existing model
      pmMatch = model[i];

      // before we update the models, we need to classify
      SortModelsByKey();

      // find which models account for the bg by summing the
      //  model's weights until we exceed <bgT>
      double weightSum = 0.0;
      for(j=0; j<K_MODELS; j++)
      {
         weightSum += model[j]->weight;

         if (weightSum > bgT) break;
      }

      // determine if the matched model is part of the bg or fg
      lastCategory = PC_Background;
      for(i=j+1; i<K_MODELS; i++)
      {
         if (pmMatch == model[i])
         {
            lastCategory = PC_Foreground;
            break;
         }
      }

      // now that we've classified, we need to update the models
      // zot: do we also update the weights if nothing matched?

      // update the weights (we normalize later)
      d = 1.0 - alpha;
      for(i=0; i<K_MODELS; i++)
      {
         if (model[i] == pmMatch)
         {
            // this is the matched model, so M = 1
            model[i]->weight = d * model[i]->weight + alpha;
         }
         else{
            // this is not the matched model, so M = 0
            model[i]->weight = d * model[i]->weight;
         }
      }

      // update the parameters for the matched model
      //rho = alpha * Gauss(pmMatch);
      rho = alpha * 0.75;
      d = 1.0 - rho;

      // first we update the mean
      for(i=0; i<3; i++)
      {
         pmMatch->mean[i] = (d * pmMatch->mean[i]) + (rho * lastPixelValue[i]);
      }

      // and then we update the variance
      pmMatch->var = (d * pmMatch->var) + (rho * pmMatch->dist2);
      pmMatch->dev = sqrt(pmMatch->var);

   }
   else{
      // we didn't match anything
      pmMatch = GetLeastProbableModel();    

      // replace least probable model with the current data
      pmMatch->weight = InitialWeight;
      for(j=0; j<3; j++) pmMatch->mean[j] = lastPixelValue[j];
      pmMatch->dev = InitialDeviation;
      pmMatch->var = InitialVariance;
      
      // we assume the 'new' model is foreground
      lastCategory = PC_Foreground;
   }   

   // we adjusted the weights during the update, so we need to normalize
   NormalizeWeights();

   return lastCategory;
}

//////////////////////////////////////////////////////////////////////

/*
 All weights should sum to one
 */
void PixelProcess::NormalizeWeights()
{
   double total = 0.0;
   int i;
   for(i=0; i<K_MODELS; i++) total += model[i]->weight;
   for(i=0; i<K_MODELS; i++) model[i]->weight /= total;
}

//////////////////////////////////////////////////////////////////////

/*
 Sort by descending key, where key = weight(i) / stddev(i)
 */
void PixelProcess::SortModelsByKey()
{   
   int i, j;
   double t;
   double key[K_MODELS];
   PixelModel *pm;

   for(i=0; i<K_MODELS; i++)
   {
      key[i] = model[i]->weight / model[i]->dev;
   }

   // zot: selection sort is crap

   for(i=0; i<K_MODELS-1; i++)
   {
      for(j=i+1; j<K_MODELS; j++)
      {
         if (key[i] < key[j])
         {
            t = key[i]; key[i] = key[j]; key[j] = t;
            pm = model[i]; model[i] = model[j]; model[j] = pm;
         }
      }
   }
}

//////////////////////////////////////////////////////////////////////

/*
 Sort by ascending variance
 */
void PixelProcess::SortModelsByVariance()
{   
   int i, j;
   PixelModel *pm;

   // zot: selection sort is (still) crap

   for(i=0; i<K_MODELS-1; i++)
   {
      for(j=i+1; j<K_MODELS; j++)
      {
         if (model[i]->var > model[j]->var)
         {
            pm = model[i]; model[i] = model[j]; model[j] = pm;
         }
      }
   }
}

//////////////////////////////////////////////////////////////////////

PixelCategory PixelProcess::GetLastCategory()
{
   return lastCategory;
}

//////////////////////////////////////////////////////////////////////

/*
 Least probable model is that with the lowest weight
 */
PixelModel* PixelProcess::GetLeastProbableModel()
{
   int i, j = 0;
   double m = model[0]->weight;
   for(i=1; i<K_MODELS; i++)
   {
      if (model[i]->weight <= m)
      {
         m = model[i]->weight;
         j = i;
      }
   }
   return model[j];
}

//////////////////////////////////////////////////////////////////////


//////////////////////////////////////////////////////////////////////////

PixelModel* PixelProcess::GetMatchedModelToProcessGaussian()
{

	return pmMatch;
}
double PixelProcess::ToGray(BYTE cr, BYTE cg, BYTE cb)
{
	return GREY_ALPHA*cr + GREY_BETA*cg + GREY_GAMMA*cb;

}

double PixelProcess::LastPixelGrayValue()
{
	return GREY_ALPHA*lastPixelValue[0] + GREY_BETA*lastPixelValue[1] + GREY_GAMMA*lastPixelValue[2];

}

void PixelProcess::GetGrayGauss_Mu_Var(/*double grayPixelValue,*/double* gray_mu,double* gray_var)
{
	
	//PixelModel* targetModel = GetMatchedModelToProcessGaussian();
	PixelModel* targetModel = model[0];
	*gray_mu = GREY_ALPHA*targetModel->mean[0] + GREY_BETA*targetModel->mean[1] + GREY_GAMMA*targetModel->mean[2];
	*gray_var = targetModel->var;

	/*
	double g_ij = 0.0f;
	double a = 3.54490770181103f;	// (2*pi)^0.5
	
	a = 1/(a * sqrt(gray_var));

	double b = -( ((grayPixelValue - gray_mu)*(grayPixelValue - gray_mu))/(2*gray_var));

	g_ij = a * exp(b);
	return g_ij;
	*/

}
